#!/usr/bin/env python
import roslib; roslib.load_manifest('stack_objects')
import rospy
from geometry_msgs.msg import PointStamped
from pr2_tasks.tasks import Tasks
from pr2_tasks.pickplace import PickPlace
from pr2_tasks.tableware_detection import TablewareDetection
from pr2_tasks.pickplace_definitions import PlaceGoal
from pr2_python import base
from pr2_python.world_interface import WorldInterface
from pr2_python.planning_scene_interface import get_planning_scene_interface
import copy

def list_objects():
   global res
   det = TablewareDetection()
   head_point = PointStamped()
   head_point.header.frame_id = '/torso_lift_link'
   head_point.point.x = 0.5
   res = det.detect_objects(point_head_at=head_point)
   rospy.loginfo('Detected objects with the following labels and poses:')
   #for pg in res.pickup_goals:
       #rospy.loginfo('Label: '+str(pg.label)+', Pose:\n'+str(pg.object_pose_stamped))
   #    rospy.loginfo('Label: '+str(pg.label))
   #return

def create_unstack_list():
   global res
   global ordered_objects
   ordered_objects = []
   for pg in res.pickup_goals:
      if pg.label == 'graspable':
         ordered_objects.append(pg)
   for pg in ordered_objects:
      rospy.loginfo('Label: '+str(pg.label))  

def main():
   wi = WorldInterface()
   wi.reset()
   psi = get_planning_scene_interface()
   psi.reset()
   list_objects()
   create_unstack_list()

rospy.init_node('detect_unstack_objects_node')
main()
